package edu.wpi.first.wpilibj.positioning;

/**
 * A class storing coordinates of X, Y locations, as well as yaw,
 * pitch, roll, and speed (Of execution) as doubles.
 * @author Dylan Frese
 */
public final class Coordinates {
    
    /**
     * The amount of units between each set of coordinates in a {@link Path}
     */
    public static final double CoordinateSpacing = 6;
    
    /**
     * The doubles used to store info.
     */
    private final double x, y, yaw, pitch, roll, speed;

    /**
     * Construct a new Coordinates with the specified
     * x, y, yaw, pitch, and roll, and with a speed of 1 (full speed). Pitch, 
     * roll, and yaw must be stored as degrees, in the range of -180 to 179.
     * @param x The x coordinate
     * @param y The y coordinate
     * @param yaw The yaw of these coordinates
     * @param pitch The pitch of these coordinates
     * @param roll The roll of these coordinates
     * @throws IllegalArgumentException if the degrees are outside the range of
     * -180 to 179.
     */
    public Coordinates(double x, double y, double yaw, double pitch, double roll) {
        this (x, y, yaw, pitch, roll, 1);
    }
    
    /**
     * Construct a new Coordinates with the specified
     * x, y, yaw, pitch, and roll, and speed. Pitch, roll, and yaw must be
     * stored as degrees, in the range of -180 to 179.
     * @param x The x coordinate
     * @param y The y coordinate
     * @param yaw The yaw of these coordinates
     * @param pitch The pitch of these coordinates
     * @param roll The roll of these coordinates
     * @param speed The speed at which to move to these coordinates.
     * @throws IllegalArgumentException if the degrees are outside the range of
     * -180 to 179.
     */
    public Coordinates(double x, double y, double yaw, double pitch, double roll, double speed){
        this.x = x;
        this.y = y;
        this.speed = speed;
        //Convert 180+ degrees into the range of -180 to 179.
        if (pitch > 179 || pitch < -180){
            throw new IllegalArgumentException("Pitch is outside the range");
        }
        if (yaw > 179 || pitch < -180){
            throw new IllegalArgumentException("Yaw is outside the range");
        }
        if (roll > 179 || pitch < -180){
            throw new IllegalArgumentException("Roll is outside the range");
        }
        this.pitch = pitch;
        this.roll = roll;
        this.yaw = yaw;
    }

    /**
     * Get the pitch of these coordinates.
     * @return the pitch of these coordinates.
     */
    public double getPitch() {
        return pitch;
    }
    
    /**
     * Get the X-coordinate.
     * @return the X-coordinate.
     */
    public double getX() {
        return x;
    }

    /**
     * Get the Y-coordinate.
     * @return the Y-coordinate.
     */
    public double getY() {
        return y;
    }

    /**
     * Get the yaw of these coordinates.
     * @return the yaw of these coordinates.
     */
    public double getYaw() {
        return yaw;
    }
    
    /**
     * Get the roll of these coordinates.
     * @return the roll of these coordinates.
     */
    public double getRoll() {
        return roll;
    }
    
    /**
     * Get the speed at which to move to these coordinates.
     * @return the speed at which to move to these coordinates.
     */
    public double getSpeed() {
        return speed;
    }
    
}
